//------------------------------------------------------------------------------
//
//  Integrator Drive Service
//
//  Written Jan-2008 by Trevor Taylor, Software Technology, Australia
//
//  Implementation of a generic differential drive.
//  Note that there are NO motor services. They are not necessary!
//  However, the drive state has to have motor objects.
//
//  IMPORTANT NOTE:
//  This service implements an internal port to handle all drive requests.
//  This avoids the "flooding" problem that occurs if you implement motors
//  and then use the generic differential drive implementation provided
//  with MRDS instead of creating your own drive service.
//
//  It also simulates DriveDistance and RotateDegrees using timers. These
//  operations are NOT accurate, but at least you don't have a "Not
//  Implemented" exception thrown at you, and they are handy.
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Services.SubscriptionManager;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using W3C.Soap;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using dssphttp = Microsoft.Dss.Core.DsspHttp;
using xml = System.Xml;


// Add references for motor and physical model
using motor = Microsoft.Robotics.Services.Motor.Proxy;
using model = Microsoft.Robotics.PhysicalModel.Proxy;

// For HttpGet
using Microsoft.Dss.Core.DsspHttp;
// For HttpStatusCode
using System.Net;

// Refer to other parts of the solution
using ProMRDS.Robotics.Integrator;

using brick = ProMRDS.Robotics.GenericBrick.Proxy;


namespace ProMRDS.Robotics.Integrator.Drive
{
    /// <summary>
    /// Integrator Differential Drive (Two motors that function together)
    /// </summary>
    [DisplayName("Integrator Generic Differential Drive")]
    [Description("Provides access to Integrator Differential Drive (Two motors)")]
    [Contract(Contract.Identifier)]
    [AlternateContract("http://schemas.microsoft.com/robotics/2006/05/drive.html")]
    public class IntegratorDriveService : DsspServiceBase
    {
        /// <summary>
        /// Integrator Main Port
        /// </summary>
        [ServicePort("/integratordrive", AllowMultipleInstances = true)]
        private drive.DriveOperations _mainPort = new Microsoft.Robotics.Services.Drive.Proxy.DriveOperations();

        /// <summary>
        /// Integrator Drive Service State
        /// </summary>
        [InitialStatePartner(Optional=true, ServiceUri="Integrator.Drive.Config.xml")]
        private drive.DriveDifferentialTwoWheelState _state = new Microsoft.Robotics.Services.Drive.Proxy.DriveDifferentialTwoWheelState();

        // Set up the XSLT file for displaying the state
        [EmbeddedResource("ProMRDS.Robotics.Integrator.Resources.IntegratorDrive.xslt")]
        string _transform = null;

        /// <summary>
        /// Subscription Manager Partner
        /// </summary>
        [Partner("SubMgr", Contract = "http://schemas.microsoft.com/xw/2005/01/subscriptionmanager.html", CreationPolicy = PartnerCreationPolicy.CreateAlways, Optional = false)]
        private SubscriptionManagerPort _subMgrPort = new Microsoft.Dss.Services.SubscriptionManager.SubscriptionManagerPort();

        /// <summary>
        /// Integrator Brick Partner
        /// </summary>
        // TT Jul-2008 - Change in July CTP
        //[Partner("Integrator", Contract = "http://www.promrds.com/contracts/2008/01/integrator.html", CreationPolicy = PartnerCreationPolicy.UseExisting, Optional = false)]
        [Partner("Integrator", Contract = brick.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting, Optional = false)]
        //private IntegratorOperations _brickPort = new IntegratorOperations();
        private brick.GenericBrickOperations _brickPort = new brick.GenericBrickOperations();

        // Used for driving the motors. Always execute the newest pending drive request.
        private Port<drive.SetDrivePower> _internalDrivePowerPort = new Port<drive.SetDrivePower>();

        // Used for stopping the motors after a timed motion
        private Port<StopMotion> _internalStopPort = new Port<StopMotion>();
        // Flag to indicate that a Stop is pending for a timed motion
        bool _ackPending = false;
        // Counter for ACKs (Every ACK has a unique ID)
        int _ackCounter = 0;
        // Pending response port (Used to cancel outstanding requests)
        PortSet<DefaultUpdateResponseType, Fault> _ackPort = null;

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public IntegratorDriveService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

            // Now add ourselves to the directory
            base.Start();

            // Wait one time for an InternalDrivePower command
            Activate(Arbiter.ReceiveWithIterator<drive.SetDrivePower>(false, _internalDrivePowerPort, InternalDrivePowerHandler));

            // Set up the Stop Motion handler which must be in the Exclusive group
            // along with the other drive requests
            MainPortInterleave.CombineWith(
                new Interleave(
                    new ExclusiveReceiverGroup(
                        Arbiter.ReceiveWithIterator<StopMotion>(true, _internalStopPort, StopMotionHandler)
                    ),
                    new ConcurrentReceiverGroup()
                )
            );
        }

        #region Initialization

        /// <summary>
        /// Initialize the drive state
        /// </summary>
        private void InitializeState()
        {
            // Create a new state if there is none
            if (_state == null)
            {
                _state = new drive.DriveDifferentialTwoWheelState();
            }

            // Sanity check (or reset) all of the state properties

            _state.DriveState = drive.DriveState.Stopped;
            _state.IsEnabled = false;

            // The distance between wheels is not exact because the
            // actual contact points of the wheels with the ground
            // might not be in the middle of the wheels.
            // The distance is about 12.5cm from edge to edge.
            if (_state.DistanceBetweenWheels == 0.0)
                _state.DistanceBetweenWheels = 0.125;

            if (_state.LeftWheel == null)
                _state.LeftWheel = new motor.WheeledMotorState();
            if (_state.RightWheel == null)
                _state.RightWheel = new motor.WheeledMotorState();

            _state.LeftWheel.WheelSpeed = 0.0;
            _state.RightWheel.WheelSpeed = 0.0;

            if (_state.LeftWheel.GearRatio == 0.0)
                _state.LeftWheel.GearRatio = 1.0;
            if (_state.RightWheel.GearRatio == 0.0)
                _state.RightWheel.GearRatio = 1.0;

            // Wheel radius is about 3.5cm, but this cannot
            // be measured very accurately and there will also
            // be effects as the wheels sink into carpet, etc.
            if (_state.LeftWheel.Radius == 0.0)
                _state.LeftWheel.Radius = 0.035;
            if (_state.RightWheel.Radius == 0.0)
                _state.RightWheel.Radius = 0.035;

            model.Pose p = new model.Pose();
            p.Position = new model.Vector3(-0.0625f, 0.035f, 0);
            p.Orientation = new model.Quaternion(-1, 0, 0, 0);
            if (_state.LeftWheel.MotorState == null)
                _state.LeftWheel.MotorState = new motor.MotorState("Left Motor", 1, 1.0, false, p);
            if (_state.LeftWheel.MotorState.HardwareIdentifier == 0)
                _state.LeftWheel.MotorState.HardwareIdentifier = 1;
            if (string.IsNullOrEmpty(_state.LeftWheel.MotorState.Name))
                _state.LeftWheel.MotorState.Name = "Left Motor";
            if (_state.LeftWheel.MotorState.PowerScalingFactor == 0.0)
                _state.LeftWheel.MotorState.PowerScalingFactor = 1.0;

            p.Position = new model.Vector3(0.0625f, 0.035f, 0);
            p.Orientation = new model.Quaternion(1, 0, 0, 0);
            if (_state.RightWheel.MotorState == null)
                _state.RightWheel.MotorState = new motor.MotorState("Right Motor", 2, 1.0, true, p);
            if (_state.RightWheel.MotorState.HardwareIdentifier == 0)
                _state.RightWheel.MotorState.HardwareIdentifier = 2;
            if (string.IsNullOrEmpty(_state.RightWheel.MotorState.Name))
                _state.RightWheel.MotorState.Name = "Right Motor";
            if (_state.RightWheel.MotorState.PowerScalingFactor == 0.0)
                _state.RightWheel.MotorState.PowerScalingFactor = 1.0;

            SaveState(_state);
        }

        #endregion

        #region Operation Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(drive.Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }
        
        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(Microsoft.Dss.Core.DsspHttp.HttpGet httpGet)
        {
            // Display the state using an XSLT transform
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );

            yield break;
        }
        
        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpPostHandler(dssphttp.HttpPost submit)
        {
            // TODO: Implement Submit operations here.
            throw new NotImplementedException("HttpPost is not implemented");
        }
        
        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReliableSubscribeHandler(drive.ReliableSubscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        
        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SubscribeHandler(drive.Subscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        
        /// <summary>
        /// Update Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateHandler(drive.Update update)
        {
            // TODO: Implement Update operations here.
            throw new NotImplementedException("Update is not implemented");
        }
        
        /// <summary>
        /// EnableDrive Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> EnableDriveHandler(drive.EnableDrive update)
        {
            // Stop now if drive is to be disabled
            if (!update.Body.Enable)
            {
                ClearPendingStop();
                // Internal drive power is not affected by Enable
                _internalDrivePowerPort.Post(new drive.SetDrivePower(new drive.SetDrivePowerRequest(0, 0)));
            }

            _state.IsEnabled = update.Body.Enable;
            _state.TimeStamp = DateTime.Now;
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }
        
        /// <summary>
        /// SetDrivePower Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower update)
        {
            // Return a Fault if the drive is not enabled
            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            /*
            Port<drive.SetDrivePower> p = _mainPort;
            if (p.ItemCount > 10)
                Console.WriteLine("Backlog of " + p.ItemCount);
            */

            if (update.Body.LeftWheelPower == 0 && update.Body.RightWheelPower == 0)
                _state.DriveState = drive.DriveState.Stopped;
            else
                _state.DriveState = drive.DriveState.DrivePower;

            // Pass the request to the internal handler
            _internalDrivePowerPort.Post(update);
        }
        
        /// <summary>
        /// SetDriveSpeed Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDriveSpeedHandler(drive.SetDriveSpeed update)
        {
            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            // TODO: Convert meters/second to power based on radius of wheels.
            // There has to be another "fudge factor" that converts m/s to a power setting
            // Many of the available services do not implement speed because it is too hard!

            // Force this via the internal port so that it gets queued
            // with the rest of the drive requests
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Just copy speed across to power
            // Obviously this is NOT correct, but whatcha gonna do?
            power.Body.LeftWheelPower = update.Body.LeftWheelSpeed;
            power.Body.RightWheelPower = update.Body.RightWheelSpeed;
            // Copy the response port too so that the sender will get a reply
            power.ResponsePort = update.ResponsePort;

            if (power.Body.LeftWheelPower == 0 && power.Body.RightWheelPower == 0)
                _state.DriveState = drive.DriveState.Stopped;
            else
                _state.DriveState = drive.DriveState.DriveSpeed;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(power);
        }

        #region Timed Moves

        /// <summary>
        /// CalculateTimeToMove
        /// </summary>
        /// <param name="distance">Distance to travel</param>
        /// <param name="power">Drive power to use</param>
        /// <returns>Number of milliseconds</returns>
        public int CalculateTimeToMove(double distance, double power)
        {
            // Drive distance is in meters, but we have to convert it to a
            // time delay in milliseconds. The "magic" number here was found
            // by trial and error using a 30cm drive distance. It will change
            // as the batteries run down and due to a variety of other factors.
            return (int)(Math.Abs(distance) * 3250.0);
        }


        /// <summary>
        /// RotateDegrees Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees update)
        {
            // This handler is basically the same as DriveDistance except for the
            // calculation of the distance. It could use DriveDistance, but that
            // is an unnecessary overhead.
            // See DriveDistance for more comments.

            // Time delay for the motion
            int delay;
            // Speeds for wheels to move at
            double leftSpeed;
            double rightSpeed;
            // Distance wheels will travel during rotation
            double arcDistance;

            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            // Check for zero degrees first because we don't want the wheels to jerk
            // if the robot is supposed to go nowhere!
            if (update.Body.Degrees == 0.0)
            {
                update.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }
            else if (update.Body.Degrees < 0.0)
            {
                // Note that the speeds are opposite to make the robot
                // rotate on the spot
                leftSpeed = update.Body.Power;
                rightSpeed = -update.Body.Power;
            }
            else
            {
                leftSpeed = -update.Body.Power;
                rightSpeed = update.Body.Power;
            }

            // Calculate the distance that the wheels must travel for the given
            // turn angle. They will move around the circumference of a circle
            // with a diameter equal to the wheel base. Each 360 degrees of rotation
            // is one full circumference.
            arcDistance = Math.Abs(update.Body.Degrees) * _state.DistanceBetweenWheels * Math.PI / 360;

            // Calculate the delay time
            delay = CalculateTimeToMove(arcDistance, update.Body.Power);

            // Start the motors running (in opposite directions)
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Set the power
            power.Body.LeftWheelPower = leftSpeed;
            power.Body.RightWheelPower = rightSpeed;
            // Do NOT respond to this message (Happens later on Stop)
            power.ResponsePort = null;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(power);
            _state.DriveState = drive.DriveState.RotateDegrees;

            // Set the timer and forget about it
            SetStopTimer(delay, update.ResponsePort);
        }
        
        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance update)
        {
            // Drive Distance can be used to move a specified distance.
            //
            // Use a timer -- start the motors, wait, stop the motors.
            //
            // The problem is that using timers on Windows is not
            // reliable. On top of that, there are transmission delays
            // when sending commands, to it is not possible to make very
            // accurate moves this way.
            //

            // Time delay for the motion
            int delay;
            // Speed to move at
            // NOTE: "speed" is really the power because robot does not implement both.
            // Power values should be between 0 and 1.
            // The speed should be calculated based on the wheel radius and power,
            // but this is not implemented.
            double speed = Math.Abs(update.Body.Power);
            // Distance to travel
            // NOTE: Microsoft says that the Distance should always be POSITIVE and
            // that you use a negative Power to move backwards. I believe that a
            // convention of negative distances should be acceptable. Therefore
            // the code therefore checks both possibilities. This leads to the
            // strange case of a negative distance AND power, this is forwards!
            double distance = update.Body.Distance * Math.Sign(update.Body.Power);

            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Drive is not enabled")));
                yield break;
            }

            // All motion requests must do this first!
            ClearPendingStop();

            // Check for zero distance first because we don't want the wheels to jerk
            // if the robot is supposed to go nowhere!
            if (distance == 0.0)
            {
                update.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }
            else if (distance < 0.0)
            {
                // Speed needs to be negative if distance is negative
                speed = -speed;
            }
            else
            {
                // Speed remains the same (positive)
            }

            // Calculate the delay time
            delay = CalculateTimeToMove(distance, speed);

            // Start the motors running!
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Set the power
            power.Body.LeftWheelPower = speed;
            power.Body.RightWheelPower = speed;
            // Do NOT respond to this message (Happens later on Stop)
            power.ResponsePort = null;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(power);
            _state.DriveState = drive.DriveState.DriveDistance;

            // Set the timer and forget about it
            SetStopTimer(delay, update.ResponsePort);
        }

        #endregion

        /// <summary>
        /// AllStop Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> AllStopHandler(drive.AllStop update)
        {
            // All motion requests must do this first!
            ClearPendingStop();

            // Kill all outstanding requests!
            // Because this is Exclusive, there cannot be another drive operation
            // in progress
            _internalDrivePowerPort.Clear();

            _state.DriveState = drive.DriveState.Stopped;

            // Explicitly send a stop command
            yield return Arbiter.Choice(_brickPort.SetDrivePower(new drive.SetDrivePowerRequest(0.0, 0.0)),
                delegate(DefaultUpdateResponseType ok)
                {
                    update.ResponsePort.Post(ok);
                },
                delegate(Fault fault)
                {
                    update.ResponsePort.Post(fault);
                });
        }

        #endregion

        #region Internal Handlers

        // Issue a fault if a pending Stop was terminated and
        // clear the flag
        void ClearPendingStop()
        {
            if (_ackPending)
            {
                _ackPending = false;
                Fault f;
                f = Fault.FromException(new Exception("Stop after timed motion aborted"));
                _ackPort.Post(f);
                _ackPort = null;
            }
        }

        // Set a timer to post a Stop later
        void SetStopTimer(int delay, PortSet<DefaultUpdateResponseType, Fault> p)
        {
            int num;

            // Classic case of "should never happen"
            ClearPendingStop();

            // Increment the ACK counter to make it unique and
            // keep a copy for later
            _ackCounter++;
            num = _ackCounter;
            // Remember the current response port in case it has
            // to be cancelled
            _ackPort = p;
            // Enable ACK
            _ackPending = true;

            // Setup a delegate for the specified time period
            // It will post a StopMotion message when it fires
            Activate(Arbiter.Receive(
                false,
                TimeoutPort(delay),
                delegate(DateTime timeout)
                {
                    // Send ourselves a Stop request using the ACK number
                    _internalStopPort.PostUnknownType(new StopMotion(num));
                }
            ));
        }

        /// <summary>
        /// Internal Stop Motion Handler
        /// Called in response to a Stop message when a timed motion finishes
        /// </summary>
        /// <param name="stop"></param>
        /// <returns></returns>
        public virtual IEnumerator<ITask> StopMotionHandler(StopMotion stop)
        {
            // Somebody beat us to it!
            if (!_ackPending)
                yield break;

            // Another request has arrived and overridden this one
            // so there is still one pending, but it is not us
            if (stop.Body.AckNumber != _ackCounter)
                yield break;

            // Now stop the robot and post a response back to say the move is complete
            drive.SetDrivePower power = new drive.SetDrivePower();
            // Set the power
            power.Body.LeftWheelPower = 0;
            power.Body.RightWheelPower = 0;
            // Set the response port
            power.ResponsePort = _ackPort;

            // Use the internal handler for throttling
            _internalDrivePowerPort.Post(power);
            _state.DriveState = drive.DriveState.Stopped;

            // Clear the ACK
            _ackPending = false;
            _ackPort = null;

        }

        /// <summary>
        /// Internal Drive Power Handler
        /// Processes the most recent Drive Power command
        /// When complete, self activate for the next internal command
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <returns></returns>
        public virtual IEnumerator<ITask> InternalDrivePowerHandler(drive.SetDrivePower power)
        {
            try
            {
                // Take a snapshot of the number of pending commands at the time
                // we entered this routine.
                // This will prevent a livelock which can occur if we try to
                // process the queue until it is empty, but the inbound queue
                // is growing at the same rate as we are pulling off the queue.
                int pendingCommands = _internalDrivePowerPort.ItemCount;

                // If newer commands have been issued, send success responses
                // to the older commands and move to the latest one
                drive.SetDrivePower newerUpdate;
                while (pendingCommands > 0)
                {
                    if (_internalDrivePowerPort.Test(out newerUpdate))
                    {
                        // Timed motion requests do not have a response initially,
                        // that comes later when the motors are stopped
                        if (power.ResponsePort != null)
                            power.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                        power = newerUpdate;
                    }
                    pendingCommands--;
                }

                yield return Arbiter.Choice(_brickPort.SetDrivePower(new drive.SetDrivePowerRequest(power.Body.LeftWheelPower, power.Body.RightWheelPower)),
                    delegate(DefaultUpdateResponseType ok)
                    {
                        _state.LeftWheel.MotorState.CurrentPower = power.Body.LeftWheelPower;
                        _state.RightWheel.MotorState.CurrentPower = power.Body.RightWheelPower;
                        if (power.ResponsePort != null)
                            power.ResponsePort.Post(ok);
                    },
                    delegate(Fault fault)
                    {
                        if (power.ResponsePort != null)
                            power.ResponsePort.Post(fault);
                    }
                );

            }
            finally
            {
                // Wait one time for the next InternalDrivePower command
                Activate(Arbiter.ReceiveWithIterator(false, _internalDrivePowerPort, InternalDrivePowerHandler));
            }
            yield break;
        }

        #endregion

    }
}
